#include "Kalmanfilter.h"
#include "UPRE_GLOBAL_DEFINE.h"
float DT35_distance[2] = {0};
float Real_value,Real_value_1,filteredValue,filteredValue_2;
uint16_t ADC_value[2]={0};
uint8_t cnt=0;
KalmanFilter filter,filter_2;
void initKalmanFilter_1(KalmanFilter *filter, float initialP, float initialQ, float initialR) {
    filter->prevData = 0;
    filter->p = initialP;
    filter->q = initialQ;
    filter->r = initialR;
    filter->kGain = 0;
}
void updateKalmanParams_1(KalmanFilter *filter, float newQ, float newR) {
    filter->q = newQ;
    filter->r = newR;
}
//һ�׿������˲�
float kalmanFilter_1(KalmanFilter *filter, float inData) {
    filter->p = filter->p + filter->q;
    filter->kGain = filter->p / (filter->p + filter->r);
    inData = filter->prevData + (filter->kGain * (inData - filter->prevData));
    filter->p = (1 - filter->kGain) * filter->p;
    filter->prevData = inData;
    return inData;
}
void DT35_TIMCallback(TIM_HandleTypeDef *htim)
{
			if (htim->Instance == DT35_TIM.Instance) {	
			HAL_ADC_Start_DMA(DT35_ADC,(uint32_t*)&ADC_value,2);
	}
			
}
	
void DT35_ADCCallback(ADC_HandleTypeDef * hadc)
{

		if(hadc==DT35_ADC)
	{
		static float sum,sum_1;
		Real_value=(float)(ADC_value[0])/4096*3.3;
		Real_value_1=(float)(ADC_value[1])/4096*3.3;
		filteredValue = kalmanFilter_1(&filter, Real_value);
		//Dt35_1ԭʼ��ȡ������
		float m=Real_value/2.20*3.99+0.0565;
		//Dt35_1�˲��������
		float k_m=filteredValue/2.20*3.980+0.0405;
		sum=(sum+k_m)/2;
		//printf("Read: %f,%f\r\n",mm,km);
		filteredValue_2 = kalmanFilter_1(&filter_2, Real_value_1);
		//Dt35_2ԭʼ��ȡ������
		float m_1=Real_value_1/2.20*3.98+0.0525;
		//Dt35_2�˲��������
	  float k_m_1=filteredValue_2/2.20*3.98+0.0525;
		sum_1=(sum_1+k_m_1)/2;
		DT35_distance[0]=sum * 1000.f;
		DT35_distance[1]=sum_1 * 1000.f;
//		printf("Read: %f,%f\r\n",DT35_distance[0],DT35_distance[1]);
//		printf("\r\n");

	}
}
void DT35_Init()
{
  initKalmanFilter_1(&filter, 1, 0.005, 0.01);//3/4ԽСԽ���Ź���ֵ����֮���Ų���ֵ
	initKalmanFilter_1(&filter_2, 1, 0.005, 0.01);
	filteredValue = kalmanFilter_1(&filter, 2.20);
	filteredValue_2 = kalmanFilter_1(&filter_2, 2.20);
	HAL_ADC_Start_DMA(DT35_ADC,(uint32_t*)&ADC_value,2);
	HAL_TIM_Base_Start_IT(&DT35_TIM);
}